#ifndef __MYIMU_H_
#define __MYIMU_H_ 1

#include <math.h>
#include "MyConfig.h"
#include "MySensors.h"

//======================================================================================//
//====================http://www.starlino.com/imu_kalman_arduino.html===================//
//======================================================================================//
#if 0
char firstSample;					//marks first sample

struct {
	char inpInvert[INPUT_COUNT];    // bits 0..5 invert input
	int zeroLevel[INPUT_COUNT];     // 0..2 accelerometer zero level (mV) @ 0 G
	// 3..5 gyro zero level (mV) @ 0 deg/s
	int inpSens[INPUT_COUNT];       // 0..2 accelerometer input sensitivity (mv/g)
	// 3..5 gyro input sensitivity (mV/deg/ms)
	float wGyro;					// gyro weight/smoothing factor
} config;

//Notation "w" stands for one of the axes, so for example RwAcc[0],RwAcc[1],RwAcc[2] means RxAcc,RyAcc,RzAcc
//Variables below must be global (their previous value is used in getEstimatedInclination)
float RwEst[3];     //Rw estimated from combining RwAcc and RwGyro
//unsigned long lastMicros;

//Variables below don't need to be global but we expose them for debug purposes
unsigned long interval; //interval since previous analog samples
float RwAcc[3];         //projection of normalized gravitation force vector on x/y/z axis, as measured by accelerometer
float RwGyro[3];        //Rw obtained from last estimated value and gyro movement
float Awz[2];           //angles between projection of R on XZ/YZ plane and Z axis (deg)

void loop();
void 5dofIMU_Init();
#endif
//======================================================================================//
//======================================================================================//

// some constants
#define SMOOTH_SCALE			10		// gia tri on dinh
#define SMOOTH_SCALE_H			5		// gia tri on dinh

#define Kp						2.0f 	// proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki						0.005f	// integral gain governs rate of convergence of gyroscope biases
//#define kpAcc	0.2f
//#define kiAcc	0.005f
//#define kpMag	2.0f
//#define kiMag	0.005f
float q0, q1, q2, q3;					// quaternion elements representing the estimated orientation
float exInt, eyInt, ezInt;				// scaled integral error
unsigned long lastUpdate, now;			// sample period expressed in milliseconds
float halfT;							// 1/2 the sample period expressed in seconds

extern int32_t h;						// do cao

extern float q[4];
extern float Omega[3];
extern float val[9];					// dung cho ham main truyen du lieu cho PC
//extern float testmag[3];
extern unsigned char setup_status;

extern float HC_Yaw, HC_Pitch, HC_Roll;
void MyIMU_Init();
void MyIMU_AHRSupdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
float MyIMU_GetHeading();
void MyIMU_getQuaternion(float * q);
void MyIMU_getYawPitchRoll(float * ypr);
//void MyIMU_getValues(float * values);
//void MyIMU_getEuler(float * angles);

int32_t SmoothH(int32_t val);

#endif
